// EKF SLAM WITH KNOWN CORRESPONDENCES

// Variaveis de Entrada
// Média do Estado Anterior -> Inicializa com a posição inicial do Robo (X_robo,Y_robo,Theta_robo)
// Covariancia do Estado Anterior (39x39) -> Inicializada como Matriz Nula
// Controle no instante atual -> (Velocidade Transversal e Velocidade Angular)
// Leituras dos Sensores -> Conjunto de vetores (dist,theta,signature) para cada Landmark encontrado
// Estimativas das coordenadas dos Landmarks -> Lista Encadeada Simples que contém o vetor (media_x,media_y,signature) para cada Landmark do mapa

#include <kalman.h>
#include <cstdio>

/*int main (void)
{
  // Simulando os dados de entrada fornecidos pelo Player/Stage

  // Variaveis para o Pré-processamento para gerar as Listas

  double dist_blob[6] = {0,4.2,1.2,6.6,6};		// Simula o que o Blob informaria sobre a distância linear para os Landmarks ele está vendo
  double theta_blob[6] = {0.0,2.0,4.0,3.0,2.0,0.0};	// Simula o que o Blob informaria sobre a distância angular para os Landmarks ele está vendo
  int signature_blob[6] = {0,1,1,1,1,0};		// Simula o que o Blob informaria sobre quais Landmarks ele está vendo
  
  // Variaveis de entrada para o Kalman Filter

  list<lista_mapa> mapa;		// Lista com as Informações do Mapa
  list<lista_sensor> sensor;		// Lista com as Leituras dos Sensores
 
  double controle[2] = {1,1};			// Simula o Controle fornecido ao Robo

  PGMATRIX Matriz_Aux1;			// Simula o Mi_(t-1)
  Matriz_Aux1 = PGMATRIX_ALLOC(3,1);
  PGMATRIX_ZEROES(Matriz_Aux1);		// Inicialização
  
  PGMATRIX Matriz_Covt;				// Simula o Cov_(t-1)
  Matriz_Covt = PGMATRIX_ALLOC(3,3);
  PGMATRIX_ZEROES(Matriz_Covt);			// Inicialização

  // Funções para gerar as Listas utilizadas no Kalman Filter - Lista_M e Lista_S

  Gera_Lista_Mapa(&mapa);
  Inicializa_Lista_Sensor(&sensor);

  //Atualiza a Lista do Sensores com as novas informações obtidas no instante "t"
  Atualiza_Lista_Sensor(&sensor,dist_blob,theta_blob,signature_blob);

  // Chama a função que Implementa o Kalman Filter
  Kalman_Filter(Matriz_Aux1, Matriz_Covt, controle, signature_blob, &mapa, &sensor);

  return 0;

}*/

// Algoritmo baseado na Pag. 314 - Probabilistic Robots ( Sebastian Thrun )
void SLAM(PGMATRIX Matriz_Mediat, PGMATRIX Matriz_Covt, double controle[], int signature[], list<lista_mapa> *Lista_M, list<lista_sensor> *Lista_S, double d_t)
{
  // Variaveis
  
  double theta_robo;
  double pos_landmark[3];
  double media_landmark[3];
  double z_landmark[3];
  double media_t[3];
  double dx_dy_q[3];
  int cont;
  
  z_landmark[2] = 0.0;
  
  PGMATRIX Matriz_Fx;  
  PGMATRIX Matriz_Gt;					
  PGMATRIX Matriz_Rt;
  PGMATRIX Matriz_Qt;
  PGMATRIX Matriz_Kt;
  PGMATRIX Matriz_Ht;

  // Alocando Memória para as Matrizes

  Matriz_Fx = PGMATRIX_ALLOC(3,39);
  Matriz_Gt = PGMATRIX_ALLOC(39,39);
  Matriz_Rt = PGMATRIX_ALLOC(3,3);

  Matriz_Qt = PGMATRIX_ALLOC(3,3);
  Matriz_Ht = PGMATRIX_ALLOC(3,39);
  Matriz_Kt = PGMATRIX_ALLOC(39,3);


  // Acochambrando para a Media

  media_t[0] = PGMATRIX_DATA(Matriz_Mediat, 1, 1);
  media_t[1] = PGMATRIX_DATA(Matriz_Mediat, 2, 1);
  media_t[2] = PGMATRIX_DATA(Matriz_Mediat, 3, 1);

  theta_robo = media_t[2];

  // Motion Update (Linhas 2-5)
  
  // Linha 2
  Gera_Matriz_Fx(Matriz_Fx);

  // Linha 3
  Atualiza_Media_Controle(media_t, controle, theta_robo, d_t);

  // Linha 4
  Gera_Matriz_Gt(Matriz_Gt, Matriz_Fx, controle, theta_robo, d_t);

  // Linha 5
  Gera_Matriz_Rt(Matriz_Rt); 
  Atualiza_Covariancia_Controle(Matriz_Gt, Matriz_Fx, Matriz_Rt, Matriz_Covt);
  
  // Measurement Update (Linhas 6 - 22)

  // Linha 6
  Gera_Matriz_Qt(Matriz_Qt);
  
  // Linha 7 - 20
       
  for(cont = 0; cont<12; cont++)	
  {
    // For all observed features - Linha 10
    if (signature[cont] == 1)
    {
      Ler_Lista_Mapa(Lista_M,cont,pos_landmark);
      Ler_Lista_Sensor(Lista_S,cont,z_landmark);

      // Linha 9 - 11 -> Se o landmark "cont" nunca estiver visto antes
      if(pos_landmark[2] != 1) 
      {
	    Landmark_First_Time(media_landmark,media_t,z_landmark);
	    Atualiza_Flag_Land(Lista_M,cont,media_landmark);
      }
      else
      {
        media_landmark[0] = pos_landmark[0];
        media_landmark[1] = pos_landmark[1];       
      }

      // Linha 12 e Linha 13
      Gera_Delta_e_Q(dx_dy_q,media_t,media_landmark);

      // Linha 15 - 16
      Gera_Matriz_Ht(Matriz_Ht,dx_dy_q,cont+1);

      // Linha 17
      Gera_Matriz_Kt(Matriz_Kt, Matriz_Ht, Matriz_Qt, Matriz_Covt);

      //Linha 14 e Linha 18 - 19
      Atualiza_Media_Covariancia_Sensores(media_t, dx_dy_q, z_landmark, Matriz_Kt, Matriz_Covt, Matriz_Ht, Lista_M);
    }
  }    
  
  PGMATRIX_DATA(Matriz_Mediat, 1, 1) = media_t[0];
  PGMATRIX_DATA(Matriz_Mediat, 2, 1) = media_t[1];
  PGMATRIX_DATA(Matriz_Mediat, 3, 1) = media_t[2];

  // Liberando Memoria

  PGMATRIX_FREE(Matriz_Fx);  
  PGMATRIX_FREE(Matriz_Gt);
  PGMATRIX_FREE(Matriz_Rt);
  PGMATRIX_FREE(Matriz_Qt);
  PGMATRIX_FREE(Matriz_Kt);
  PGMATRIX_FREE(Matriz_Ht);
  
}

void Gera_Matriz_Fx(PGMATRIX Matriz_Fx)	
{
  // Variaveis

  PGMATRIX_ZEROES(Matriz_Fx);

  PGMATRIX_DATA(Matriz_Fx, 1, 1) = 1.0;
  PGMATRIX_DATA(Matriz_Fx, 2, 2) = 1.0;
  PGMATRIX_DATA(Matriz_Fx, 3, 3) = 1.0;

}

void Atualiza_Media_Controle(double media_t[], double controle[], double theta_robo, double d_t)	
{
  // Variaveis
 
  double aux1,aux2;

  aux1 = (controle[0]/controle[1]);
  aux2 = (controle[1]*d_t);

  // Atualizando a Media_t a partir do Controle fornecido (Control Update - Parte 1)
  if(controle[1] != 0 )
  {
    media_t[0] = media_t[0] + ( ((-1)*aux1*sin(theta_robo)) + (aux1*sin(theta_robo + aux2)) );
    media_t[1] = media_t[1] + ( (aux1*cos(theta_robo)) - (aux1*cos(theta_robo + aux2)) );
  }
  
  // Tratando o caso em que Velocidade Angular seja igual a zero
  else
  {
  
    media_t[0] = media_t[0] + controle[0]*cos(theta_robo)*(d_t);
    media_t[1] = media_t[1] + controle[0]*sin(theta_robo)*(d_t);
  
  }
 
  media_t[2] = media_t[2] + aux2;

}

void Gera_Matriz_Gt(PGMATRIX Matriz_Gt, PGMATRIX Matriz_Fx, double controle[], double theta_robo, double d_t)	
{
  // Variaveis
  
  double aux1,aux2;

  PGMATRIX Matriz_Fx_Trans;
  PGMATRIX Matriz_Aux;
  PGMATRIX Matriz_Aux1;
  PGMATRIX Matriz_Aux2;
  PGMATRIX Matriz_I;

  Matriz_Fx_Trans = PGMATRIX_ALLOC(39,3);
  Matriz_Aux = PGMATRIX_ALLOC(3,3);
  Matriz_Aux1 = PGMATRIX_ALLOC(3,39);
  Matriz_Aux2 = PGMATRIX_ALLOC(39,39);
  Matriz_I = PGMATRIX_ALLOC(39,39);  


  aux1 = (controle[0])/(controle[1]);	// aux1 = (velocidade transversal)/(velocidade angular)
  aux2 = (controle[1]*d_t);

  // Preenchendo a Matriz Aux

  PGMATRIX_ZEROES(Matriz_Aux);
 
  if(controle[1] != 0 )
  { 
    PGMATRIX_DATA(Matriz_Aux, 1, 3) = (aux1)*(cos(theta_robo + aux2)) - aux1*(cos(theta_robo));
    PGMATRIX_DATA(Matriz_Aux, 2, 3) = (aux1)*(sin(theta_robo + aux2)) - aux1*(sin(theta_robo));
  }
  
  // Tratando o caso em que Velocidade Angular seja igual a zero
  else
  {
    PGMATRIX_DATA(Matriz_Aux, 1, 3) = (-1)*(controle[0])*(sin(theta_robo))*d_t;
    PGMATRIX_DATA(Matriz_Aux, 2, 3) = (controle[0])*(cos(theta_robo))*d_t;
  }
  
  // Determinando Gt

  PGMATRIX_TRANSPOSE_COPY(Matriz_Fx_Trans, Matriz_Fx);
  PGMATRIX_IDENTITY(Matriz_I); 

  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Aux2, Matriz_Fx_Trans, Matriz_Aux, Matriz_Fx, Matriz_Aux1);
  PGMATRIX_ADD_COPY(Matriz_Gt, Matriz_I, Matriz_Aux2);

  // Liberando Memória

  PGMATRIX_FREE(Matriz_Fx_Trans);
  PGMATRIX_FREE(Matriz_Aux);
  PGMATRIX_FREE(Matriz_Aux1);
  PGMATRIX_FREE(Matriz_Aux2);
  PGMATRIX_FREE(Matriz_I);

}

void Gera_Matriz_Rt(PGMATRIX Matriz_Rt)	
{
  // Preenchendo a Matriz Rt

  PGMATRIX_ZEROES(Matriz_Rt);

  PGMATRIX_DATA(Matriz_Rt, 1, 1) = pow(alpha_x,2);
  PGMATRIX_DATA(Matriz_Rt, 2, 2) = pow(alpha_y,2);
  PGMATRIX_DATA(Matriz_Rt, 3, 3) = pow(alpha_theta,2);

}

void Atualiza_Covariancia_Controle(PGMATRIX Matriz_Gt, PGMATRIX Matriz_Fx, PGMATRIX Matriz_Rt, PGMATRIX Matriz_Covt) 
{
  PGMATRIX Matriz_Gt_Trans;
  PGMATRIX Matriz_Aux;
  PGMATRIX Matriz_Aux1;

  PGMATRIX Matriz_Fx_Trans;
  PGMATRIX Matriz_Aux2;
  PGMATRIX Matriz_Aux3;
  
  // Alocando Memoria para as Matrizes Transpostas e para as Auxiliares
  
  Matriz_Gt_Trans = PGMATRIX_ALLOC(39,39);
  Matriz_Aux = PGMATRIX_ALLOC(39,39);
  Matriz_Aux1 = PGMATRIX_ALLOC(39,39);

  Matriz_Fx_Trans = PGMATRIX_ALLOC(39,3);
  Matriz_Aux2 = PGMATRIX_ALLOC(39,39);  
  Matriz_Aux3 = PGMATRIX_ALLOC(3,39); 
    
  // Calculando as Matriz Transpostas de Gt e de Fx
  
  PGMATRIX_TRANSPOSE_COPY(Matriz_Gt_Trans, Matriz_Gt);
  PGMATRIX_TRANSPOSE_COPY(Matriz_Fx_Trans, Matriz_Fx);  
  
  // Atualizando a Cov_t a partir do Controle fornecido (Control Update - Parte 2)

  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Aux, Matriz_Gt, Matriz_Covt, Matriz_Gt_Trans, Matriz_Aux1); 	// Aux = Gt*Cov_(t-1)*Gt_Trans

  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Aux2, Matriz_Fx_Trans, Matriz_Rt, Matriz_Fx, Matriz_Aux3);	// Aux2 = Ft*Mt*Ft_Trans

  PGMATRIX_ADD_COPY(Matriz_Covt, Matriz_Aux, Matriz_Aux2);						// Cov_t = Aux + Aux2
  
  // Liberando a Memória
  
  PGMATRIX_FREE(Matriz_Gt_Trans);
  PGMATRIX_FREE(Matriz_Fx_Trans);
  PGMATRIX_FREE(Matriz_Aux);
  PGMATRIX_FREE(Matriz_Aux1);
  PGMATRIX_FREE(Matriz_Aux2);
  PGMATRIX_FREE(Matriz_Aux3);
  
}

void Gera_Matriz_Qt(PGMATRIX Matriz_Qt) 
{
  // Preenchendo a Matriz Qt

  PGMATRIX_ZEROES(Matriz_Qt);

  PGMATRIX_DATA(Matriz_Qt, 1, 1) = pow(sigma_r,2);
  PGMATRIX_DATA(Matriz_Qt, 2, 2) = pow(sigma_phi,2);
  PGMATRIX_DATA(Matriz_Qt, 3, 3) = pow(sigma_s,2);
  
}

void Landmark_First_Time(double media_landmark[], double media_t[], double z_landmark[])
{

  media_landmark[0] = media_t[0] + z_landmark[0]*cos(z_landmark[1] + media_t[2]);
  media_landmark[1] = media_t[1] + z_landmark[0]*sin(z_landmark[1] + media_t[2]);

}

void Gera_Delta_e_Q(double dx_dy_q[], double media_t[], double media_landmark[])
{
  // Determinando Delta_X
  
  dx_dy_q[0] = media_landmark[0] - media_t[0];
  
  // Determinando Delta_Y
  
  dx_dy_q[1] = media_landmark[1] - media_t[1];

  // Determinando Q
 
  dx_dy_q[2] = dx_dy_q[0]*dx_dy_q[0] + dx_dy_q[1]*dx_dy_q[1];

}

void Gera_Matriz_Ht(PGMATRIX Matriz_Ht, double dx_dy_q[], int j)
{
  // Variaveis

  double raiz_q;

  raiz_q = sqrt(dx_dy_q[2]);

  // Matrizes Auxiliares

  PGMATRIX Matriz_Fx_land;
  PGMATRIX Matriz_Aux;

  Matriz_Fx_land = PGMATRIX_ALLOC(6,39);
  Matriz_Aux = PGMATRIX_ALLOC(3,6);  

  // Determinando a Matriz Fx_land

  PGMATRIX_ZEROES(Matriz_Fx_land);

  PGMATRIX_DATA(Matriz_Fx_land, 1, 1) = 1.0;
  PGMATRIX_DATA(Matriz_Fx_land, 2, 2) = 1.0;
  PGMATRIX_DATA(Matriz_Fx_land, 3, 3) = 1.0;

  PGMATRIX_DATA(Matriz_Fx_land, 4, 3*j + 1) = 1.0;
  PGMATRIX_DATA(Matriz_Fx_land, 5, 3*j + 2) = 1.0;
  PGMATRIX_DATA(Matriz_Fx_land, 6, 3*j + 3) = 1.0;

  // Determinando a Matriz Aux

  PGMATRIX_ZEROES(Matriz_Aux);

  PGMATRIX_DATA(Matriz_Aux, 1, 1) = ((-1)*(raiz_q)*(dx_dy_q[0]))/(dx_dy_q[2]);
  PGMATRIX_DATA(Matriz_Aux, 1, 2) = ((-1)*(raiz_q)*(dx_dy_q[1]))/(dx_dy_q[2]);
  PGMATRIX_DATA(Matriz_Aux, 1, 4) = ((raiz_q)*(dx_dy_q[0]))/(dx_dy_q[2]);
  PGMATRIX_DATA(Matriz_Aux, 1, 5) = ((raiz_q)*(dx_dy_q[1]))/(dx_dy_q[2]);

  PGMATRIX_DATA(Matriz_Aux, 2, 1) = (dx_dy_q[1])/(dx_dy_q[2]);
  PGMATRIX_DATA(Matriz_Aux, 2, 2) = ((-1)*(dx_dy_q[0]))/(dx_dy_q[2]);
  PGMATRIX_DATA(Matriz_Aux, 2, 3) = ((-1)*(dx_dy_q[2]))/(dx_dy_q[2]);
  PGMATRIX_DATA(Matriz_Aux, 2, 4) = ((-1)*(dx_dy_q[1]))/(dx_dy_q[2]); 
  PGMATRIX_DATA(Matriz_Aux, 2, 5) = (dx_dy_q[0])/(dx_dy_q[2]);

  PGMATRIX_DATA(Matriz_Aux, 3, 6) = (dx_dy_q[2])/(dx_dy_q[2]);

  // Determinando a Matriz Ht

  PGMATRIX_MULTIPLY_COPY(Matriz_Ht, Matriz_Aux, Matriz_Fx_land);

  // Liberando memória

  PGMATRIX_FREE(Matriz_Fx_land);
  PGMATRIX_FREE(Matriz_Aux);

}

void Gera_Matriz_Kt(PGMATRIX Matriz_Kt, PGMATRIX Matriz_Ht, PGMATRIX Matriz_Qt, PGMATRIX Matriz_Covt)	
{
  // Variaveis
  PGMATRIX Matriz_St;
  PGMATRIX Matriz_St_Inv;
  PGMATRIX Matriz_Ht_Trans;
  PGMATRIX Matriz_Aux;
  PGMATRIX Matriz_Aux1;
  PGMATRIX Matriz_Aux2;

  // Alocando memoria
  Matriz_St = PGMATRIX_ALLOC(3,3);
  Matriz_St_Inv = PGMATRIX_ALLOC(3,3);
  Matriz_Ht_Trans = PGMATRIX_ALLOC(39,3);
  Matriz_Aux = PGMATRIX_ALLOC(3,3);
  Matriz_Aux1 = PGMATRIX_ALLOC(39,3);
  Matriz_Aux2 = PGMATRIX_ALLOC(39,3);  

  // Construindo a matriz St
  PGMATRIX_TRANSPOSE_COPY(Matriz_Ht_Trans, Matriz_Ht); 
  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Aux, Matriz_Ht, Matriz_Covt, Matriz_Ht_Trans, Matriz_Aux1); 	
  PGMATRIX_ADD_COPY(Matriz_St, Matriz_Aux, Matriz_Qt);						       

  // Construindo a matriz Kt
  PGMATRIX_INVERSE_COPY(Matriz_St_Inv, Matriz_St);
  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Kt, Matriz_Covt, Matriz_Ht_Trans, Matriz_St_Inv, Matriz_Aux2); 	

  // Liberando a memoria alocada
  PGMATRIX_FREE(Matriz_Aux);
  PGMATRIX_FREE(Matriz_Aux1);
  PGMATRIX_FREE(Matriz_Aux2);
  PGMATRIX_FREE(Matriz_St);
  PGMATRIX_FREE(Matriz_St_Inv);
  PGMATRIX_FREE(Matriz_Ht_Trans);

}

void Atualiza_Media_Covariancia_Sensores(double media_t[], double dx_dy_q[], double z_landmark[], PGMATRIX Matriz_Kt, PGMATRIX Matriz_Covt, PGMATRIX Matriz_Ht, list<lista_mapa> *Lista_Mapa)
{
  // Variaveis

  PGMATRIX Matriz_Zchapeu;
  PGMATRIX Matriz_Zt;
  PGMATRIX Matriz_I;

  PGMATRIX Matriz_Aux;
  PGMATRIX Matriz_Aux1;
  PGMATRIX Matriz_Aux2;
  PGMATRIX Matriz_Aux3;
  PGMATRIX Matriz_Aux4;

  // Alocando memoria
  Matriz_Zchapeu = PGMATRIX_ALLOC(3,1);
  Matriz_Zt = PGMATRIX_ALLOC(3,1);
  Matriz_I = PGMATRIX_ALLOC(39,39);  

  Matriz_Aux = PGMATRIX_ALLOC(3,1);
  Matriz_Aux1 = PGMATRIX_ALLOC(39,1);

  Matriz_Aux2 = PGMATRIX_ALLOC(39,39);
  Matriz_Aux3 = PGMATRIX_ALLOC(39,39);
  Matriz_Aux4 = PGMATRIX_ALLOC(39,39);

  // Construindo a Matriz Zchapeu

  PGMATRIX_DATA(Matriz_Zchapeu, 1, 1) = sqrt(dx_dy_q[2]); 

  PGMATRIX_DATA(Matriz_Zchapeu, 2, 1) = atan2(dx_dy_q[1],dx_dy_q[0]) - media_t[2];
  
  PGMATRIX_DATA(Matriz_Zchapeu, 3, 1) = z_landmark[2];
  
  // Construindo a Matriz Zt
  
  PGMATRIX_DATA(Matriz_Zt, 1, 1) = z_landmark[0]; 
  PGMATRIX_DATA(Matriz_Zt, 2, 1) = z_landmark[1]; 
  PGMATRIX_DATA(Matriz_Zt, 3, 1) = z_landmark[2];
  
  // Atualizando a Média com as Informações dos Sensores
  
  PGMATRIX_SUBSTRACT_COPY(Matriz_Aux, Matriz_Zt, Matriz_Zchapeu);
  PGMATRIX_MULTIPLY_COPY(Matriz_Aux1, Matriz_Kt, Matriz_Aux);

  media_t[0] = media_t[0] + PGMATRIX_DATA(Matriz_Aux1, 1, 1);
  media_t[1] = media_t[1] + PGMATRIX_DATA(Matriz_Aux1, 2, 1);
  media_t[2] = media_t[2] + PGMATRIX_DATA(Matriz_Aux1, 3, 1);
  
  //std::cout << "Aux1 (x,y,theta)= " << PGMATRIX_DATA(Matriz_Aux1, 1, 1) << " " << PGMATRIX_DATA(Matriz_Aux1, 2, 1) << " " << PGMATRIX_DATA(Matriz_Aux1, 3, 1) << std::endl;
  //std::cout << PGMATRIX_DATA(Matriz_Aux1, 4, 1) << " " << PGMATRIX_DATA(Matriz_Aux1, 5, 1) << " " << PGMATRIX_DATA(Matriz_Aux1, 6, 1) << std::endl;

  // Atualizando as Melhores Estimativas para as Posicoes dos Landmarks

  Atualiza_Lista_Mapa(Lista_Mapa, Matriz_Aux1);
  
  // Atualizando a Convariancia com as Informações dos Sensores
  
  PGMATRIX_IDENTITY(Matriz_I);
  PGMATRIX_MULTIPLY_COPY(Matriz_Aux4, Matriz_Kt, Matriz_Ht);  
  PGMATRIX_SUBSTRACT_COPY(Matriz_Aux2, Matriz_I, Matriz_Aux4);  

  PGMATRIX_MULTIPLY_COPY(Matriz_Aux3, Matriz_Aux2, Matriz_Covt);
  PGMATRIX_COPY(Matriz_Covt, Matriz_Aux3); 

  // Liberando a memoria alocada
  
  PGMATRIX_FREE(Matriz_Zchapeu); 
  PGMATRIX_FREE(Matriz_Zt);
  PGMATRIX_FREE(Matriz_I);
  PGMATRIX_FREE(Matriz_Aux);
  PGMATRIX_FREE(Matriz_Aux1);
  PGMATRIX_FREE(Matriz_Aux2);
  PGMATRIX_FREE(Matriz_Aux3);
  PGMATRIX_FREE(Matriz_Aux4);

}
